#include <comm_ipc.hpp>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <unistd.h>

CommMsgQueue::CommMsgQueue() {
	msgqKey = -1;
	msgqID = -1;
}
CommMsgQueue::~CommMsgQueue() {
}

int CommMsgQueue::Create(char *path, char proj) {
	int qid;

	key_t msgkey = ftok(path, proj);
	if (msgkey == -1) {
		return -1;
	}

	if ((qid = msgget(msgkey, IPC_CREAT | 0660)) == -1) {
		return -1;
	}

	msgqKey = msgkey;
	msgqID = qid;
	return (qid);
}

int CommMsgQueue::Open(key_t keyval) {
	return keyval;
}

int CommMsgQueue::Close(void) {
	int ret = -1;
	if (msgqID != -1) {
		ret = msgctl(msgqID, IPC_RMID, 0);
		if (ret >= 0) {
			msgqKey = -1;
		}
	}

	return ret;
}

int CommMsgQueue::Write(void *msgp, int msgsz, long int mtype) {
	int ret = -1;
	struct msgbuf *msg = (struct msgbuf *) malloc(MSG_MAX_SIZE);

	if (msgsz > MSG_MAX_SIZE) {
		return ret;
	}

	msg->mtype = mtype;
	if (msgsz > 0) {
		memcpy(&msg->mtext[0], msgp, msgsz);
	}

	if (msgsz <= MSG_MAX_SIZE && msgqID != -1) {
		ret = msgsnd(msgqID, msg, msgsz + sizeof(long int), IPC_NOWAIT);
		free(msg);
		return ret;
	}

	free(msg);
	return -1;
}
int CommMsgQueue::Write(void *msgp, int msgsz, long int mtype, int mtimeoutms) {
	int ret = -1;
	struct msgbuf *msg = (struct msgbuf *) malloc(MSG_MAX_SIZE);
	memset(msg, '0', MSG_MAX_SIZE);

	if (msgsz > MSG_MAX_SIZE) {
		return ret;
	}

	msg->mtype = mtype;
	if (msgsz > 0 && msgsz < MSG_MAX_SIZE) {
		memcpy(&msg->mtext[0], msgp, msgsz);
	}

	if (msgsz <= MSG_MAX_SIZE && msgqID != -1) {
		if (mtimeoutms < 0) {
			ret = msgsnd(msgqID, msg, msgsz + sizeof(long int), 0);
		}
		while (mtimeoutms >= 0) {
			ret = msgsnd(msgqID, msg, msgsz + sizeof(long int), IPC_NOWAIT);
			if (ret == -1) {
				if (mtimeoutms >= 10) {
					usleep(10 * 1000);
					mtimeoutms -= 10;
				} else {
					usleep(1 * 1000);
					mtimeoutms--;
				}
			} else {
				free(msg);
				return ret;
			}
		}
	}

	free(msg);
	return ret;
}

int CommMsgQueue::Read(void *msgp, int msgsz, long int mtype) {
	if (msgqID != -1 && msgsz >= (int) sizeof(long int)) {
		msgsz -= sizeof(long int);

		return msgrcv(msgqID, msgp, msgsz, mtype, IPC_NOWAIT);
	}

	return -1;
}

int CommMsgQueue::Read(void *msgp, int msgsz, long int mtype, int mtimeoutms) {
	int ret = -1;
	if (msgqID != -1 && msgsz >= (int) sizeof(long int)) {
		msgsz -= sizeof(long int);
		if (mtimeoutms < 0) {
			ret = msgrcv(msgqID, (struct msgbuf *) msgp, msgsz, mtype, 0);
		}

		while (mtimeoutms >= 0) {
			ret = msgrcv(msgqID, (struct msgbuf *) msgp, msgsz, mtype, IPC_NOWAIT);
			if (ret == -1) {
				if (mtimeoutms >= 10) {
					usleep(10 * 1000);
					mtimeoutms -= 10;
				} else {
					usleep(1 * 1000);
					mtimeoutms--;
				}
			} else {
				return ret;
			}
		}
	}

	return ret;
}
